#include "floatconverter.h"

#include <bitset>
#include <string>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>
#include <vector>

#ifndef _MSC_VER
// not VC++
#elif _MSC_VER < 1400
// older than VC++ 2005
#elif _MSC_VER < 1500
// VC++ 2005
#elif _MSC_VER < 1600
// VC++ 2008
#elif _MSC_VER < 1700
// VC++ 2010
#define _MSVC12
#else
// Future versions
#endif

const int TWOS_COMPLEMENT_MAX_8 = 127;

// start indexes are 0-based
const int MIL1750A_FPORTION_START = 8;
const int MIL1750A_FPORTION_SIZE = 23;
const int MIL1750A_EXPONENT_START = 0;
const int MIL1750A_EXPONENT_SIZE = 8;
const int MIL1750A_SIGN_INDEX = 31;

const int IEEE754_EXPONENT_START = 23;
const int IEEE754_EXPONENT_SIZE = 8;
const int IEEE754_FPORTION_START = 0;
const int IEEE754_FPORTION_SIZE = 23;
const int IEEE754_SIGN_INDEX = 31;

/**
* Converts a 32 bit Mil-Std 1750A float to IEEE float
*/
std::bitset<32> FloatConverter::convert1750AFloatToIEEEFloat(unsigned long bytes)
{
    std::bitset<32> floatIEEE;

// compiler bug in visual studio 2010
#ifdef _MSC_VER == 1600
	std::bitset<32> float1750A(static_cast<unsigned long long>(bytes));
#else
	std::bitset<32> float1750A(bytes);
#endif

    std::bitset<8> exponentIEEE;
    std::bitset<8> exponent1750A; // same as 2's complement

    // convert fportion to bitset - size 23 does not include sign
    std::bitset<23> fportionBitset;

    bool sign = false;

    if ( bytes != 0)
    {
	    // get exponent and convert from 2's complement to 127 offset binary representation
	    // exponent is between and including bits 0 to 7
	    for ( int i = 0; i < MIL1750A_EXPONENT_SIZE; i++)
	        exponent1750A[i] = float1750A[i];
	
	    // MILSTD1750A uses 2's complement
	    // int exponentValue = exponentIEEE.to_ulong() - 127;
	    int iexponentValue1750A = 0;
	
	    // calculate exponent value for positive sign
	    for (int i = 0; i <= 6; i++)
	    {
	        if ( exponent1750A[i] )
	            iexponentValue1750A += std::pow(float(2),int(i));
	    }
	
	    // MSB = 0 | 1? if negative sign adjust exponent value
	    if ( exponent1750A[IEEE754_EXPONENT_SIZE - 1] == true )
	    {
	        // negative number
	        iexponentValue1750A -= 128;
	    }

        // convert exponent value to 127 offset bias representation
	    int exponentValue = iexponentValue1750A + 127;
	
	    // IEEE exponent value is - 1 from 1750A float see
	    // Telemetry Standard RCC Document 106-09, Appendix O, April 2009, section 4.0
	    exponentValue--;
	
	    int remainder = exponentValue;
	
	    for ( int i = IEEE754_EXPONENT_SIZE - 1; i >= 0; i-- )
	    {
	        exponentIEEE[i] = ((remainder/int(std::pow(float(2),i))) != 0);
	        remainder = remainder % int(std::pow(float(2),i));
	    }
	
	    // rearrange the position of the exponent
	
	    // sign remains the same
        sign = float1750A[MIL1750A_SIGN_INDEX];
	    floatIEEE[IEEE754_SIGN_INDEX] = float1750A[MIL1750A_SIGN_INDEX];
	
	    // exponent bits for IEEE are from 23 to 30 inclusive (0 based indexing)
        for ( int i = 0; i < IEEE754_EXPONENT_SIZE; i++ )
            floatIEEE[IEEE754_EXPONENT_START + i] = exponentIEEE[i];
	
	    // fractional part is between bits 8 to 30 inclusive of 1750A
	    // shift fractional part left to account for change of -1
	    // in exponent value
        for ( int i = 0; i < IEEE754_FPORTION_SIZE; i++)
            fportionBitset[i] = float1750A[MIL1750A_FPORTION_START + i];

        if(sign)
        {
            // implies fportion part in 1750A float is in two's complement
            // therefore - 1, and invert
#ifdef _MSC_VER == 1600
			std::bitset<23> temp(static_cast<unsigned long long>(fportionBitset.to_ullong() - 1));
#else
			std::bitset<23> temp(fportionBitset.to_ullong() - 1);
#endif
			assign(fportionBitset, temp);
            fportionBitset.flip();
        }

        // add fportion to IEEE float - need to account for decrease by 1 in exponent
        // implies left shift - only copy 22 bits across and skip last signed bit of
        // two's complement number
        for ( int i = 0; i < IEEE754_FPORTION_SIZE - 1; i++ )
            floatIEEE[i + 1] = fportionBitset[i];
	
	    // account for skipped bit
	    floatIEEE[IEEE754_FPORTION_START] = 0;
    } 
    else
    {
        floatIEEE.reset();
    }

    // return IEEE float bitset
    return floatIEEE;
}
//-----------------------------------------------------------------------------

std::bitset<32> FloatConverter::convertIEEEFloatTo1750AFloat(unsigned long bytes)
{

#ifdef _MSC_VER == 1600 
	std::bitset<32> floatIEEE(static_cast<unsigned long long>(bytes));
#else
	std::bitset<32> floatIEEE(bytes);
#endif

    std::bitset<32> float1750A;
    std::bitset<8> exponentIEEE;
    std::bitset<8> exponent1750A; // 2's complement

    // convert fportion to bitset - size 23 does not include sign
    std::bitset<23> fportionBitset;

    // dynamic sized bit vector representing the integral component
    std::vector<bool> fportionBitVector;

    // true for negative, false for positive
    bool sign = false;
    int exponent = 0;
    int integral = 0;

    if ( bytes != 0 )
    {
        
        float fTemp = 0;
        std::memcpy(&fTemp, &bytes, 4);

        // extract float into its fractional and integral parts
        float fractionalFloatPart, integralFloatPart;
        fractionalFloatPart = std::modf(fTemp, &integralFloatPart);

        // extract sign and convert integral part to integer
        if ( fractionalFloatPart >= 0 )
        {
            integral = int(integralFloatPart);
            sign = false;
        }
        else
        {
            integral = -int(integralFloatPart);
            sign = true;
        }

        // convert integral component and add to bit vector
        int twosComplement = integral;
        while ( twosComplement > 0 )
        {
            fportionBitVector.push_back((twosComplement%2) != 0);
            twosComplement /= 2;
        }

        // convert fractional component and add to bit vector
        int i = 0;
        while (fractionalFloatPart != 0)
        {
            fractionalFloatPart = std::modf((fractionalFloatPart*2), &integralFloatPart);
            fportionBitVector.insert(fportionBitVector.begin(), integralFloatPart != 0);
            i++;
        }

        for ( size_t i = 0; i < fportionBitVector.size(); i++)
            fportionBitset[fportionBitset.size() - fportionBitVector.size() + i] = fportionBitVector[i];

        // convert fportion to 2's complement - if -ve invert then add 1
        if(sign)
        {
            fportionBitset.flip();

#ifdef _MSC_VER == 1600
			std::bitset<23> temp(static_cast<unsigned long long>(fportionBitset.to_ullong() + 1));
#else
			std::bitset<23> temp(fportionBitset.to_ullong() + 1);
#endif
            assign(fportionBitset, temp);
        }

	    // get exponent and convert to 2's complement exponent
	    // exponent is between and including bits 23 to 30
	    // see http://en.wikipedia.org/wiki/Single_precision_floating-point_format
	    for ( int i = 0; i < IEEE754_EXPONENT_SIZE; i++)
	        exponentIEEE[i] = floatIEEE[i + IEEE754_EXPONENT_START];
	
	    // IEEE standard uses a 127 bias
	    int exponentValue = exponentIEEE.to_ulong() - 127;
	
	    // 1750A exponent value is + 1 see
	    // Telemetry Standard RCC Document 106-09, Appendix O, April 2009, section 4.0
	    exponentValue++;

        // shift fportion left by exponent value if integral = 0
        if (integral == 0)
            fportionBitset <<= std::abs(exponentValue);
	
	    if ( exponentValue > TWOS_COMPLEMENT_MAX_8 )
	        throw std::runtime_error("Error converting IEEE Float to MILSTD1750A Float: Invalid Exponent.");
	
	    // convert to 2's complement
	    int denominator = TWOS_COMPLEMENT_MAX_8 + 1;
	    int numerator = exponentValue;
	
	    for ( int i = 0; i < MIL1750A_EXPONENT_SIZE; i++)
	    {
	        exponent1750A[MIL1750A_EXPONENT_SIZE - 1 - i] = ((numerator/denominator) != 0);
	        numerator = numerator % denominator;
	        denominator = denominator/2;
	    }
	
	    // was the exponent a negative number
	    if ( exponentValue < 0 )
	    {
	        // to convert to two's complement, flip then add 1
	        exponent1750A.flip();

#ifdef _MSC_VER == 1600
			std::bitset<8> temp(static_cast<unsigned long long>(exponent1750A.to_ulong() + 1));
#else
			std::bitset<8> temp(exponent1750A.to_ulong() + 1);
#endif
	        assign(exponent1750A, temp);// = exponent1750A.to_ulong() + 1;
	    }
	
	    // create mil-std-1750A float
	
	    // insert sign, S
	    float1750A[31] = sign;

        // insert fractional part
        for ( int i = 0; i < MIL1750A_FPORTION_SIZE; i++ )
            float1750A[i + MIL1750A_FPORTION_START] = fportionBitset[i];

        // insert exponent
        for ( int i = 0; i < MIL1750A_EXPONENT_SIZE; i++ )
            float1750A[i] = exponent1750A[i];
    } 
    else
    {
        float1750A.reset();
    }

    // return string representing hex value
    return float1750A;
}
//-----------------------------------------------------------------------------

std::string FloatConverter::get1750AFloatStringFromIEEEFloat(unsigned long bytesIEEEFloat)
{
    return convertIEEEFloatTo1750AFloat(bytesIEEEFloat).to_string();
}
//-----------------------------------------------------------------------------

unsigned long FloatConverter::get1750AFloatBytesFromIEEEFloat(unsigned long bytesIEEEFloat)
{
    return convertIEEEFloatTo1750AFloat(bytesIEEEFloat).to_ulong();
}
//-----------------------------------------------------------------------------

unsigned long FloatConverter::getIEEEFloatBytesFrom1750AFloat(unsigned long bytes1750AFloat)
{
    return convert1750AFloatToIEEEFloat(bytes1750AFloat).to_ulong();
}
//-----------------------------------------------------------------------------

std::string FloatConverter::getIEEEFloatStringFrom1750AFloat(unsigned long bytes1750Float)
{
    return convert1750AFloatToIEEEFloat(bytes1750Float).to_string();
}
//-----------------------------------------------------------------------------

std::string FloatConverter::getIEEEFloatString(float f)
{
    unsigned long data = 0;
    
    // 4 bytes in a 32 bit float
    std::memcpy(&data, &f, 4);

    std::bitset<32> bits(static_cast<unsigned long long>(data));
    std::string bitString = 
        bits.to_string< char, std::char_traits<char>, std::allocator<char> >();

    return bitString;
}
//-----------------------------------------------------------------------------

std::string FloatConverter::getIEEEFloatHexString(float f)
{
    // 4 bytes in a 32 bit float
    unsigned char data[4];
    std::stringstream ss;
    std::string str = "";

    // 4 bytes in a 32 bit float
    std::memcpy(data, &f, 4);
    for (int i = 3; i >= 0; i--)
    {
        // MSB at largest index (big-endian)
        // mask out the sign extended bits with & 0xff
        ss << std::hex << std::setfill('0') << std::setw(2) << (data[i] & 0xff);
    }
    
    str += ss.str();
    str += "\n";
    return str;
}
//-----------------------------------------------------------------------------

std::string FloatConverter::get1750AFloatString(unsigned long)
{
    return "";
}
//-----------------------------------------------------------------------------

unsigned long FloatConverter::getIEEEFloatBytesFromDecFloat( unsigned long bytesDecFloat )
{
	return 0;
}
//-----------------------------------------------------------------------------

unsigned long FloatConverter::geDecFloatBytesFromIEEEFloat( unsigned long bytesIEEEFloat )
{
	return 0;
}
//-----------------------------------------------------------------------------

template<int bits>
void FloatConverter::assign( std::bitset<bits>& lhs, const std::bitset<bits>& rhs )
{
	for(int i = 0; i < bits; i++ )
		lhs[i] = rhs[i];
}
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
